#include "cartographer/rcm/imu/pose_integrate.h"

namespace cartographer
{
    PoseIntegrator::PoseIntegrator()
    {
    }

    int PoseIntegrator::Integrate(const std::vector<sensor::ImuData> imuSet,
                                  const common::Time &time_start,
                                  const common::Time &time_end)
    {
        integrator_ = std::make_shared<ImuPreintegrate>(last_bias_);
        int dataSize = imuSet.size();
        ImuMiddleCalculator calc(time_start, time_end, dataSize);
        // dataSize - 1 very important
        for (int i = 0; i < (dataSize - 1); i++)
        {
            ImuDataAlias imuCurr = imuSet[i];
            ImuDataAlias imuNext = imuSet[i + 1];
            if (calc.Calculate(imuCurr, imuNext, i) < 1)
                continue;
            integrator_->Update(calc.acc.cast<float>(), calc.angular.cast<float>(), calc.tstep);
        }
        return 1;
    }

    void PoseIntegrator::Predict(const TimedPose &lastPose, const common::Time &currTime, TimedPose &currPose)
    {
        auto duration = common::ToSeconds(currTime - lastPose.time);
        if (duration < integrator_->dT)
            duration = integrator_->dT;
        // 上时刻PVR
        const Eigen::Quaterniond rotation = lastPose.pose.rotation();
        const Eigen::Vector3d Pwb1 = lastPose.pose.translation();
        const Eigen::Matrix3d Rwb1 = rotation.normalized().toRotationMatrix();
        const Eigen::Vector3d Vwb1 = linear_velocity_;
        const Eigen::Vector3d Gz(0, 0, -GRAVITY_VALUE);
        // PVR增量
        const auto &dtRotation = integrator_->GetDeltaRotation(last_bias_).cast<double>();
        const auto &dtPosition = integrator_->GetDeltaPosition(last_bias_).cast<double>();
        const auto &dtVelocity = integrator_->GetDeltaVelocity(last_bias_).cast<double>();
        // 当前PVR
        Eigen::Matrix3d Rwb2 = NormalizeRotation<double>(Rwb1 * dtRotation);
        Eigen::Vector3d Pwb2 = Pwb1 + Vwb1 * duration + 0.5f * duration * duration * Gz + Rwb1 * dtPosition;
        Eigen::Vector3d Vwb2 = Vwb1 + duration * Gz + Rwb1 * dtVelocity;
        linear_velocity_ = Vwb2;

        currPose.time = currTime;
        currPose.pose = transform::Rigid3d(Pwb2, Eigen::Quaterniond(Rwb2));
    }
} // namespace cartographer